#include <stdlib.h>
#include <math.h>

#include <util/msg.h>
#include <util/mem.h>

#include "../image.h"

#include "../../matrix/matrix.h"
#include "../../dsp/dsp.h"
#include "../../dsp/filters/filters.h"


static int * __create_int_sequence(int length, int start, int increment) {
	int i, inc;
	int * shift;
	
	//shift = calloc(length, sizeof (int));
	shift = mem_allocn( length, int);
	for (i = 0, inc = start; i < length; i++, inc += increment) shift[i] = inc;

	return shift;
}

static void __image_get_gradient(
		ImpMatrix * Mat,
		ImpMatrix MaskDxl, ImpMatrix MaskDxr,
		ImpMatrix MaskDyl, ImpMatrix MaskDyr,
		ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or);

void imp_image_get_gradient(ImpMatrix * Mat, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	int type;

	ImpMatrix MaskDxl;
	ImpMatrix MaskDxr;

	ImpMatrix MaskDyl;
	ImpMatrix MaskDyr;

	type = 1;

	switch (type) {
		case 1:
			MaskDxl = imp_filter_get("sobel", 3, 'x', 1);
			MaskDxr = imp_filter_get("sobel", 3, 'x', 2);
			MaskDyl = imp_filter_get("sobel", 3, 'y', 1);
			MaskDyr = imp_filter_get("sobel", 3, 'y', 2);

			break;
		case 2:
			MaskDxl = imp_filter_get("scharr", 3, 'x', 1);
			MaskDxr = imp_filter_get("scharr", 3, 'x', 2);
			MaskDyl = imp_filter_get("scharr", 3, 'y', 1);
			MaskDyr = imp_filter_get("scharr", 3, 'y', 2);

			break;
		case 3:
			MaskDxl = imp_filter_get("farid_simoncelli", 5, 'x', 1);
			MaskDxr = imp_filter_get("farid_simoncelli", 5, 'x', 2);
			MaskDyl = imp_filter_get("farid_simoncelli", 5, 'y', 1);
			MaskDyr = imp_filter_get("farid_simoncelli", 5, 'y', 2);

			break;
		case 4:
			MaskDxl = imp_filter_get("farid_simoncelli", 7, 'x', 1);
			MaskDxr = imp_filter_get("farid_simoncelli", 7, 'x', 2);
			MaskDyl = imp_filter_get("farid_simoncelli", 7, 'y', 1);
			MaskDyr = imp_filter_get("farid_simoncelli", 7, 'y', 2);

			break;

	}

	__image_get_gradient(Mat, MaskDxl, MaskDxr, MaskDyl, MaskDyr, Dx, Dy, Mag, Or);

}

static void __image_get_gradient(
		ImpMatrix * Mat,
		ImpMatrix MaskDxl, ImpMatrix MaskDxr,
		ImpMatrix MaskDyl, ImpMatrix MaskDyr,
		ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	real * mat = NULL; // Alias to Mat data pointer 
	real * mag = NULL;
	real * or = NULL;

	ImpMatrix * Dxt = NULL;
	ImpMatrix * Dyt = NULL;
	real * dxt = NULL;
	real * dyt = NULL;

	ImpMatrix * Dxi = NULL; // Intermediary ImpMatrix Conv
	ImpMatrix * Dyi = NULL;
	real * dxi = NULL;
	real * dyi = NULL;

	real * maskx = NULL; // Alias to Mask data pointer 
	real * masky = NULL; // Alias to Mask data pointer 

	int r, c, _r, i; // Mat indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Mat rows 
	int columns; // Mat columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

//	long int matsize; // Size of Mat 
	int d; // Shifted index 

	real sumx; // Convolution sum 
	real sumy; // Convolution sum 

	/* Verification ========================================================= */
	{
		_verify(Mat);
		_verify(Dx || Dy || Mag || Or);
	}

	/* Initialization ======================================================= */
	{
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
//		matsize = Mat->size;
	}

	/* Data Allocation ====================================================== */
	{
		if (Dx || Mag || Or) Dxt = imp_matrix_new(rows, columns);
		if (Dy || Mag || Or) Dyt = imp_matrix_new(rows, columns);
		if (Mag) if (*Mag == NULL) (*Mag) = imp_matrix_new(rows, columns);
		if (Or) if (*Or == NULL) (*Or) = imp_matrix_new(rows, columns);

		if (Dxt) dxt = Dxt->data;
		if (Dyt) dyt = Dyt->data;
		if (Mag) mag = (*Mag)->data;
		if (Or) or = (*Or)->data;

		if (Dxt) Dxi = imp_matrix_new(Mat->rows, Mat->columns);
		if (Dyt) Dyi = imp_matrix_new(Mat->rows, Mat->columns);

		if (Dxt) dxi = Dxi->data;
		if (Dyt) dyi = Dyi->data;
	}

	/* Step 01: Convolve with MaskCol ======================================= */
	{
		if (Dxt) maskx = MaskDxl.data;
		if (Dyt) masky = MaskDyl.data;

		mrows = MaskDxl.rows;
		mcolumns = MaskDxl.columns;

		int rf;
		int mrc = (mrows - 1) / 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = __create_int_sequence(rows, 0, columns);

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;

				sumx = 0;
				sumy = 0;

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					d = r_shift[rf] + c;

					mi = (mrows - mr - 1);

					if (Dxt) sumx += mat[d] * maskx[mi];
					if (Dyt) sumy += mat[d] * masky[mi];

				}
				if (Dxt) dxi[i] = sumx;
				if (Dyt) dyi[i] = sumy;
			}
		}// </editor-fold>

		free(r_shift);
	}

	/* Step 02: Convolve with MaskRow ======================================= */
	{
		if (Dxt) maskx = MaskDxr.data;
		if (Dyt) masky = MaskDyr.data;

		mrows = MaskDxr.rows;
		mcolumns = MaskDxr.columns;

		int cf;
		int mcc = (mcolumns - 1) / 2;
		int _2columns_minus_2 = (columns << 1) - 2;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;

				sumx = 0;
				sumy = 0;

				for (mc = 0; mc < mcolumns; mc++) {

					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = _r + cf;

					mi = _mr + mc;
					mi = (mcolumns - mc - 1);

					if (Dxt) sumx += dxi[d] * maskx[mi];
					if (Dyt) sumy += dyi[d] * masky[mi];
				}

				if (Dxt) dxt[i] = sumx;
				if (Dyt) dyt[i] = sumy;
				if (Mag) mag[i] = sqrt(sumx * sumx + sumy * sumy);

				// Get angles -pi to +pi 
				if (Or) or[i] = atan2(sumy, sumx);
				// Convert negative angles to positive angles: Angles 0 to 2*pi 
				if (Or) if (or[i] < 0) or[i] = or[i] + 2 * M_PI;
			}
		}// </editor-fold>
	}

	{ /* Memory free ======================================================== */
		if (Dxt) imp_object_delete( Dxi );
		if (Dyt) imp_object_delete( Dyi );

		if (Dx) (*Dx) = Dxt;
		if (Dy) (*Dy) = Dyt;

		if (!Dx && Dxt) imp_object_delete( Dxt );
		if (!Dy && Dyt) imp_object_delete( Dyt );
	}

}

/*
static void OK__image_get_gradient(
		ImpImage * Gray,
		ImpMatrix MaskDxl, ImpMatrix MaskDxr,
		ImpMatrix MaskDyl, ImpMatrix MaskDyr,
		ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	ImpMatrix * _Dxt = NULL;
	ImpMatrix * _Dyt = NULL;

	ImpMatrix * _Dx = NULL;
	ImpMatrix * _Dy = NULL;

	real * _dx = NULL;
	real * _dy = NULL;

	unsigned char * gray = NULL; // Alias to Gray data pointer 
	real * _dxt = NULL;
	real * _dyt = NULL;
	real * mag = NULL;
	real * or = NULL;

	real * maskx = NULL; // Alias to Mask data pointer 
	real * masky = NULL; // Alias to Mask data pointer 

	ImpMatrix * Shift = NULL; // ImpMatrix of index shift 
	real * shift = NULL; // Alias to Shift data pointer 

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Gray rows 
	int columns; // Gray columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int imsize; // Size of Gray 
	int d; // Shifted index 

	real sumx; // Convolution sum 
	real sumy; // Convolution sum 

	_verify(Gray);
	_verify(Dx || Dy || Mag || Or);


	if (Dx || Mag || Or) _Dxt = imp_matrix_new(Gray->height, Gray->width);
	if (Dy || Mag || Or) _Dyt = imp_matrix_new(Gray->height, Gray->width);

	//if (Dx || Mag || Or) if (*Dx == NULL) (*Dx) = imp_matrix_new(Gray->height, Gray->width);
	//if (Dy || Mag || Or) if (*Dy == NULL) (*Dy) = imp_matrix_new(Gray->height, Gray->width);
	if (Mag) if (*Mag == NULL) (*Mag) = imp_matrix_new(Gray->height, Gray->width);
	if (Or) if (*Or == NULL) (*Or) = imp_matrix_new(Gray->height, Gray->width);

	if (_Dxt) _dxt = _Dxt->data;
	if (_Dyt) _dyt = _Dyt->data;
	if (Mag) mag = (*Mag)->data;
	if (Or) or = (*Or)->data;

	if (_Dxt) _Dx = imp_matrix_new(Gray->height, Gray->width);
	if (_Dyt) _Dy = imp_matrix_new(Gray->height, Gray->width);

	if (_Dxt) _dx = _Dx->data;
	if (_Dyt) _dy = _Dy->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	// Step 01: Convolve with MaskCol 
	if (_Dxt) maskx = MaskDxl.data;
	if (_Dyt) masky = MaskDyl.data;

	mrows = MaskDxl.rows;
	mcolumns = MaskDxl.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					if (_Dxt) sumx += gray[d] * maskx[mi];
					if (_Dyt) sumy += gray[d] * masky[mi];
				}
			}
			if (_Dxt) _dx[i] = sumx;
			if (_Dyt) _dy[i] = sumy;
		}
	}// </editor-fold>

	imp_object_delete(Shift);

	// Step 02: Convolve with MaskRow 
	if (_Dxt) maskx = MaskDxr.data;
	if (_Dyt) masky = MaskDyr.data;

	mrows = MaskDxr.rows;
	mcolumns = MaskDxr.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					if (_Dxt) sumx += _dx[d] * maskx[mi];
					if (_Dyt) sumy += _dy[d] * masky[mi];
				}
			}

			if (_Dxt) _dxt[i] = sumx;
			if (_Dyt) _dyt[i] = sumy;
			if (Mag) mag[i] = sqrt(sumx * sumx + sumy * sumy);

			// Angles -pi to +pi 
			if (Or) or[i] = atan2(sumy, sumx);

			// Convert negative angles to positive angles: Angles 0 to 2*pi 
			if (Or) if (or[i] < 0) or[i] = or[i] + 2 * M_PI;
		}
	}// </editor-fold>

	imp_object_delete(Shift);
	if (_Dxt) imp_object_delete(_Dx);
	if (_Dyt) imp_object_delete(_Dy);

	if (Dx) (*Dx) = _Dxt;
	if (Dy) (*Dy) = _Dyt;

	if (!Dx && _Dxt) imp_object_delete(_Dxt);
	if (!Dy && _Dyt) imp_object_delete(_Dyt);

}

 */

/*
void image_get_derivatives_sobel(ImpImage * Gray, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	ImpMatrix * _Dx;
	ImpMatrix * _Dy;

	real * _dx;
	real * _dy;

	unsigned char * gray;
	real * dx;
	real * dy;
	real * mag;
	real * or;

	ImpMatrix MaskX;
	ImpMatrix MaskY;

	real * maskx;
	real * masky;

	ImpMatrix * Shift;
	real * shift;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;

	int mrows;
	int mcolumns;

	long int imsize;
	int d;

	real sumx;
	real sumy;


	_verify(Gray);
	_verify(Dx && Dy && Mag && Or);

	if (Dx) if (*Dx == NULL) (*Dx) = imp_matrix_new(Gray->height, Gray->width);
	if (Dy) if (*Dy == NULL) (*Dy) = imp_matrix_new(Gray->height, Gray->width);
	if (Mag) if (*Mag == NULL) (*Mag) = imp_matrix_new(Gray->height, Gray->width);
	if (Or) if (*Or == NULL) (*Or) = imp_matrix_new(Gray->height, Gray->width);

	if (Dx) dx = (*Dx)->data;
	if (Dy) dy = (*Dy)->data;
	if (Mag) mag = (*Mag)->data;
	if (Or) or = (*Or)->data;

	_Dx = imp_matrix_new(Gray->height, Gray->width);
	_Dy = imp_matrix_new(Gray->height, Gray->width);

	_dx = _Dx->data;
	_dy = _Dy->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	// Step 01: Convolve with MaskCol
	//MaskX = matrix_filter_sobel_3x1_Dx();
	MaskX = imp_filter_get("sobel", 3, 'x', 1);
	maskx = MaskX.data;

	//MaskY = matrix_filter_sobel_3x1_Dy();
	MaskY = imp_filter_get("sobel", 3, 'y', 1);
	masky = MaskY.data;

	mrows = MaskX.rows;
	mcolumns = MaskX.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sumx += gray[d] * maskx[mi];
					sumy += gray[d] * masky[mi];
				}
			}
			_dx[i] = sumx;
			_dy[i] = sumy;
		}
	}// </editor-fold>

	imp_object_delete(Shift);

	// Step 02: Convolve with MaskRow
	//MaskX = matrix_filter_sobel_1x3_Dx();
	MaskX = imp_filter_get("sobel", 3, 'x', 2);
	maskx = MaskX.data;

	//MaskY = matrix_filter_sobel_1x3_Dy();
	MaskY = imp_filter_get("sobel", 3, 'y', 2);
	masky = MaskY.data;

	mrows = MaskX.rows;
	mcolumns = MaskX.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sumx += _dx[d] * maskx[mi];
					sumy += _dy[d] * masky[mi];
				}
			}

			if (Dx) dx[i] = sumx;
			if (Dy) dy[i] = sumy;
			if (Mag) mag[i] = sqrt(sumx * sumx + sumy * sumy);

			// Angles -pi to +pi 
			if (Or) or[i] = atan2(sumy, sumx);

			// Convert negative angles to positive angles
			if (or[i] < 0) {
				or[i] = or[i] + 2 * M_PI;
			}
		}
	}// </editor-fold>

	imp_object_delete(Shift);

	imp_object_delete(_Dx);
	imp_object_delete(_Dy);


}

void image_get_derivatives_farid(ImpImage * Gray, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	ImpMatrix * _Dx;
	ImpMatrix * _Dy;

	real * _dx;
	real * _dy;

	unsigned char * gray; // Alias to Gray data pointer
	real * dx;
	real * dy;
	real * mag;
	real * or;

	ImpMatrix MaskX;
	ImpMatrix MaskY;

	real * maskx; // Alias to Mask data pointer
	real * masky; // Alias to Mask data pointer

	ImpMatrix * Shift; // ImpMatrix of index shift
	real * shift; // Alias to Shift data pointer

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Gray rows 
	int columns; // Gray columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int imsize; // Size of Gray 
	int d; // Shifted index 

	real sumx; // Convolution sum 
	real sumy; // Convolution sum 


	_verify(Gray);
	_verify(Dx && Dy && Mag && Or);

	if (Dx) if (*Dx == NULL) (*Dx) = imp_matrix_new(Gray->height, Gray->width);
	if (Dy) if (*Dy == NULL) (*Dy) = imp_matrix_new(Gray->height, Gray->width);
	if (Mag) if (*Mag == NULL) (*Mag) = imp_matrix_new(Gray->height, Gray->width);
	if (Or) if (*Or == NULL) (*Or) = imp_matrix_new(Gray->height, Gray->width);

	if (Dx) dx = (*Dx)->data;
	if (Dy) dy = (*Dy)->data;
	if (Mag) mag = (*Mag)->data;
	if (Or) or = (*Or)->data;

	_Dx = imp_matrix_new(Gray->height, Gray->width);
	_Dy = imp_matrix_new(Gray->height, Gray->width);

	_dx = _Dx->data;
	_dy = _Dy->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	// Step 01: Convolve with MaskCol 
	MaskX = imp_filter_get("farid_simoncelli", 5, 'x', 1);
	maskx = MaskX.data;

	MaskY = imp_filter_get("farid_simoncelli", 5, 'y', 1);
	masky = MaskY.data;

	mrows = MaskX.rows;
	mcolumns = MaskX.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sumx += gray[d] * maskx[mi];
					sumy += gray[d] * masky[mi];
				}
			}
			_dx[i] = sumx;
			_dy[i] = sumy;
		}
	}// </editor-fold>

	imp_object_delete(Shift);

	// Step 02: Convolve with MaskRow 
	MaskX = imp_filter_get("farid_simoncelli", 5, 'x', 2);
	maskx = MaskX.data;

	MaskY = imp_filter_get("farid_simoncelli", 5, 'y', 2);
	masky = MaskY.data;

	mrows = MaskX.rows;
	mcolumns = MaskX.columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sumx += _dx[d] * maskx[mi];
					sumy += _dy[d] * masky[mi];
				}
			}

			if (Dx) dx[i] = sumx;
			if (Dy) dy[i] = sumy;
			if (Mag) mag[i] = sqrt(sumx * sumx + sumy * sumy);

			// Angles -pi to +pi 
			if (Or) or[i] = atan2(sumy, sumx);

			// Convert negative angles to positive angles 
			if (or[i] < 0) {
				// Angles 0 to 2*pi 
				or[i] = or[i] + 2 * M_PI;
			}
		}
	}// </editor-fold>

	imp_object_delete(Shift);

	imp_object_delete(_Dx);
	imp_object_delete(_Dy);


}

void image_sobel_full(ImpImage * Gray, ImpMatrix ** Dx, ImpMatrix ** Dy, ImpMatrix ** Mag, ImpMatrix ** Or) {

	unsigned char * gray; // Alias to Gray data pointer 
	real * dx;
	real * dy;
	real * mag;
	real * or;

	ImpMatrix MaskX;
	ImpMatrix MaskY;
	real * maskx; // Alias to Mask data pointer 
	real * masky; // Alias to Mask data pointer 

	ImpMatrix * Shift; // ImpMatrix of index shift 
	real * shift; // Alias to Shift data pointer 

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Gray rows 
	int columns; // Gray columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 
	long int msize; // Mask size 

	long int imsize; // Size of Gray 
	int d; // Shifted index 

	real sumx; // Convolution sum 
	real sumy; // Convolution sum 


	_verify(Gray);
	_verify(Dx && Dy && Mag && Or);

	if (Dx) if (*Dx == NULL) (*Dx) = imp_matrix_new(Gray->height, Gray->width);
	if (Dy) if (*Dy == NULL) (*Dy) = imp_matrix_new(Gray->height, Gray->width);
	if (Mag) if (*Mag == NULL) (*Mag) = imp_matrix_new(Gray->height, Gray->width);
	if (Or) if (*Or == NULL) (*Or) = imp_matrix_new(Gray->height, Gray->width);

	if (Dx) dx = (*Dx)->data;
	if (Dy) dy = (*Dy)->data;
	if (Mag) mag = (*Mag)->data;
	if (Or) or = (*Or)->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	//MaskX = matrix_filter_sobel_3x3_Dx();
	MaskX = imp_filter_get("sobel", 3, 'x', 0);
	maskx = MaskX.data;
	matrix_print(&MaskX);

	//MaskY = matrix_filter_sobel_3x3_Dy();
	MaskY = imp_filter_get("sobel", 3, 'y', 0);
	masky = MaskY.data;
	matrix_print(&MaskY);

	mrows = MaskX.rows;
	mcolumns = MaskX.columns;
	msize = MaskX.size;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sumx = 0;
			sumy = 0;

			if (r == 1 && c == 1) {
				int ii = (r + 1) * columns + (c + 1);
				ii = 0;
			}

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sumx += gray[d] * maskx[mi];
					sumy += gray[d] * masky[mi];
				}
			}

			if (Dx) dx[i] = sumx;
			if (Dy) dy[i] = sumy;
			if (Mag) mag[i] = sqrt(sumx * sumx + sumy * sumy);

			// Angles -pi to +pi 
			if (Or) or[i] = atan2(sumy, sumx);
			
			// Convert negative angles to positive angles 
			if (or[i] < 0) {
				// Angles 0 to 2*pi 
				or[i] = or[i] + 2 * M_PI;
			}
			
		}
	}
	imp_object_delete(Shift);
}
void * image_convolve_reflected_boundaries(unsigned char * data, int width, int height, real * Dx, real * Dy, real * DxDy) {

	// column vector
	real p[5] = {0.037659, 0.249153, 0.426375, 0.249153, 0.037659};
	// row vector
	real d1[5] = {0.109604, 0.276691, 0.000000, -0.276691, -0.109604};

	int mask_size, mask_center;
	int w, h, im;
	int i, _i, id;

	real * _Dx;
	real * _Dy;

	real valDx;
	real valDy;

	int wf, hf;

	_Dx = (real *) calloc(width * height, sizeof (real));
	_Dy = (real *) calloc(width * height, sizeof (real));

	mask_size = 5;
	mask_center = (mask_size - 1) / 2;

	// convolve column vector with img
	for (w = 0; w < width; w++) {

		_i = w;
		for (h = 0; h < height; h++) {
			i = _i + (h * width);

			valDx = 0.0;
			valDy = 0.0;

			for (im = 0; im < mask_size; im++) {

				hf = h + (im - mask_center);

				// reflected boundary
				if (hf < 0) hf = height + hf;
				else if (hf > height - 1) hf = hf - height;

				// final index
				id = _i + (hf * width);

				valDx += p[im] * data[ id ];
				valDy += d1[im] * data[ id ];
			}
			_Dx[i] = valDx;
			_Dy[i] = valDy;
		}
	}

	// convolve row vector with img
	for (h = 0; h < height; h++) {

		_i = h * width;

		for (w = 0; w < width; w++) {
			i = _i + w;

			valDx = 0.0;
			valDy = 0.0;

			for (im = 0; im < mask_size; im++) {

				wf = w + (im - mask_center);

				// reflected boundary
				if (wf < 0) wf = width + wf;
				else if (wf > width - 1) wf = wf - width;

				// final index
				id = _i + wf;

				valDx += d1[im] * _Dx[ id ];
				valDy += p[im] * _Dy[ id ];
			}
			Dx[i] = valDx;
			Dy[i] = valDy;
			DxDy[i] = valDx * valDy;
		}
	}

	free(_Dx);
	free(_Dy);
}

void * image_get_derivatives(unsigned char * img, int width, int height, real * Dx, real * Dy, real * DxDy) {

	// column vector
	real p[5] = {0.037659, 0.249153, 0.426375, 0.249153, 0.037659};
	// row vector
	real d1[5] = {0.109604, 0.276691, 0.000000, -0.276691, -0.109604};

	int mask_size, mask_center;
	int w, h, im;
	int i, _i, id;

	real * _Dx;
	real * _Dy;

	real valDx;
	real valDy;

	int wf, hf;

	_Dx = (real *) calloc(width * height, sizeof (real));
	_Dy = (real *) calloc(width * height, sizeof (real));

	mask_size = 5;
	mask_center = (mask_size - 1) / 2;

	// convolve column vector with img
	for (w = 0; w < width; w++) {

		_i = w;
		for (h = 0; h < height; h++) {
			i = _i + (h * width);

			valDx = 0.0;
			valDy = 0.0;

			for (im = 0; im < mask_size; im++) {

				hf = h + (im - mask_center);

				// reflected boundary
				if (hf < 0) hf = height + hf;
				else if (hf > height - 1) hf = hf - height;

				// final index
				id = _i + (hf * width);

				valDx += p[im] * img[ id ];
				valDy += d1[im] * img[ id ];
			}
			_Dx[i] = valDx;
			_Dy[i] = valDy;
		}
	}

	// convolve row vector with img
	for (h = 0; h < height; h++) {

		_i = h * width;

		for (w = 0; w < width; w++) {
			i = _i + w;

			valDx = 0.0;
			valDy = 0.0;

			for (im = 0; im < mask_size; im++) {

				wf = w + (im - mask_center);

				// reflected boundary
				if (wf < 0) wf = width + wf;
				else if (wf > width - 1) wf = wf - width;

				// final index
				id = _i + wf;

				valDx += d1[im] * _Dx[ id ];
				valDy += p[im] * _Dy[ id ];
			}
			Dx[i] = valDx;
			Dy[i] = valDy;
			DxDy[i] = valDx * valDy;
		}
	}

	free(_Dx);
	free(_Dy);
}
 



void image_conv(ImpImage * Gray, ImpMatrix * Mask, ImpMatrix ** Conv) {

	unsigned char * gray; // Alias to Gray data pointer 
	real * mask; // Alias to Mask data pointer 
	real * conv; // Alias to Conv data pointer 

	ImpMatrix * Shift; // ImpMatrix of index shift 
	real * shift; // Alias to Shift data pointer 

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Gray rows 
	int columns; // Gray columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int imsize; // Size of Gray 
	int d; // Shifted index 
	real sum; // Convolution sum 

	_verify(Gray);
	_verify(Mask);
	_verify(Conv);
	if (*Conv == NULL) (*Conv) = imp_matrix_new(Gray->height, Gray->width);

	conv = (*Conv)->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	mask = Mask->data;
	mrows = Mask->rows;
	mcolumns = Mask->columns;

	Shift = matrix_create_conv_index_shift(Mask->rows, Mask->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sum += gray[d] * mask[mi];
				}
			}
			conv[i] = sum;
		}
	}
	imp_object_delete(Shift);
}

void image_conv_2_pass(ImpImage * Gray, ImpMatrix * Mask1, ImpMatrix * Mask2, ImpMatrix ** Conv) {

	ImpMatrix * Shift; // ImpMatrix of index shift 
	real * shift; // Alias to Shift data pointer 

	ImpMatrix * _Conv;
	real * _conv;

	unsigned char * gray; // Alias to Gray data pointer 
	real * mask; // Alias to Mask data pointer 
	real * conv; // Alias to Conv data pointer 

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Gray rows 
	int columns; // Gray columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int imsize; // Size of Gray 
	int d; // Shifted index 
	real sum; // Convolution sum 

	_verify(Gray);
	_verify(Mask1);
	_verify(Mask2);
	_verify(Conv);

	_Conv = imp_matrix_new(Gray->height, Gray->width);
	_conv = _Conv->data;

	if (*Conv == NULL) (*Conv) = imp_matrix_new(Gray->height, Gray->width);

	conv = (*Conv)->data;

	gray = Gray->data;
	rows = Gray->height;
	columns = Gray->width;
	imsize = Gray->size;

	// Step 01: Setup mask1 
	mask = Mask1->data;
	mrows = Mask1->rows;
	mcolumns = Mask1->columns;

	Shift = matrix_create_conv_index_shift(Mask1->rows, Mask1->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sum += gray[d] * mask[mi];
				}
			}
			_conv[i] = sum;
		}
	}
	imp_object_delete(Shift);

	// Step 02: Setup mask2 
	mask = Mask2->data;
	mrows = Mask2->rows;
	mcolumns = Mask2->columns;

	Shift = matrix_create_conv_index_shift(Mask2->rows, Mask2->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					sum += _conv[d] * mask[mi];
				}
			}
			conv[i] = sum;
		}
	}

	imp_object_delete(Shift);
	imp_object_delete(_Conv);
}
 */
